﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

using Microsoft.Kinect;

namespace HelloKinectWorldWPF
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    
    public partial class MainWindow : Window
    {
        // the value of clour to detect edge
        int EDGE_COLOR = 10;
        // select what colour to process. default = 0( 0: red, 1: green, 2:blue)
        int SELECTED_COLOR = 0;

        Tuple<int, int> top1, top2, top3, top4;
        Tuple<int, int, int, int> lineV;
        Tuple<int, int, int, int> lineH;
        Boolean isStop = false;
        KinectSensor sensor;
        byte[] pixeldata = new byte[640*480*4];
        byte[,] red = new byte[480, 640];
        DepthImageFrame depthImage;
        Boolean getDepthImageSuccessful = false;
        int count = 0,w,h;
        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            sensor = KinectSensor.KinectSensors[0];
            sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            sensor.DepthFrameReady += DepthFrameReady;

            sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            sensor.ColorFrameReady += FrameReady;


            sensor.Start();
        }
        void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            if ((count%30==0))
            {
                depthImage = e.OpenDepthImageFrame();
                if (depthImage != null)
                    getDepthImageSuccessful = true;
                else
                    getDepthImageSuccessful = false;
            }
        }

        void FrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            count++;
            if ((count % 5 == 0) && (!isStop) && getDepthImageSuccessful)
            {
                ColorImageFrame imageFrame = e.OpenColorImageFrame();
                if ((imageFrame != null))
                {
                    BitmapSource bmap = ImageToBitmap(imageFrame);
                    if (!isStop)
                        image1.Source = bmap;

                    /*
                    count++;
                    if (count % 60 == 0)
                    {
                        System.IO.StreamWriter file = new System.IO.StreamWriter("D:\\test.txt");
                    
                        int w = Image.Width;
                        int h = Image.Height;
                        for (int xx = 0; xx < h; xx++)
                        {
                            for (int yy = 0; yy < w; yy++)
                            {
                                string value = pixeldata[xx * w + yy].ToString();

                                int length = 3 - value.Length;
                                for (int i = 0; i < length; i++)
                                    value = " " + value;

                                file.Write(value);
                                file.Write(' ');
                            }
                            file.WriteLine();
                        }
                     
                        file.WriteLine(bmap.Height);
                        file.WriteLine(bmap.Width);
                    
                        file.Close();

                        int t = 0;
                    }
                      */

                }
            }
        }
        void drawRectangle(byte[] pixeldata,int w,int leftTopX,int leftTopY,int rightTopX,int rightTopY )
        {
            for (int ii = leftTopX; ii < rightTopX; ii++)
            {
                int jj1 = leftTopY;
                int jj2 = rightTopY;
                int index1 = ii * w * 4 + jj1 * 4;
                pixeldata[index1]     = 100;
                pixeldata[index1 + 1] = 100;
                pixeldata[index1 + 2] = 100;
                pixeldata[index1 + 3] = 100;

                int index2 = ii * w * 4 + jj2 * 4;
                pixeldata[index2]     = 100;
                pixeldata[index2 + 1] = 100;
                pixeldata[index2 + 2] = 100;
                pixeldata[index2 + 3] = 100;

            }
            for (int jj = leftTopY; jj < rightTopY; jj++)
            {
                int ii1 = leftTopX;
                int ii2 = rightTopX;
                int index3 = ii1 * w * 4 + jj * 4;
                pixeldata[index3]     = 100;
                pixeldata[index3 + 1] = 100;
                pixeldata[index3 + 2] = 100;
                pixeldata[index3 + 3] = 100;

                int index4 = ii2 * w * 4 + jj * 4;
                pixeldata[index4]     = 100;
                pixeldata[index4 + 1] = 100;
                pixeldata[index4 + 2] = 100;
                pixeldata[index4 + 3] = 100;
            }
        }
        void drawLine(byte[] pixeldata, int w, Tuple<int, int> top1, Tuple<int, int> top2)
        {

            double a = top2.Item2 - top1.Item2;
            double b = top1.Item1 - top2.Item1;
            double c = top2.Item1 * top1.Item2 - top1.Item1 * top2.Item2;

            if (Math.Abs(a) < Math.Abs(b))
            {
                int minx = Math.Min(top1.Item1, top2.Item1);
                int maxx = Math.Max(top1.Item1, top2.Item1);

                for (int ii = minx; ii < maxx; ii++)
                {
                    int x = ii;
                    int y = (int)((-c - a * x) / b);
                    if (y >= w) y = w-2;
                    if (x >= 480) x = 478;
                    int index = x * w * 4 + y * 4;
                    /*
                    Console.Write(x);
                    Console.Write(' ');
                    Console.Write(y);
                    Console.WriteLine();
                    */
                    pixeldata[index] = 255;
                    pixeldata[index + 1] = 255;
                    pixeldata[index + 2] = 255;
                    pixeldata[index + 3] = 255;
                }
            }
            else
            {
                int miny = Math.Min(top1.Item2, top2.Item2);
                int maxy = Math.Max(top1.Item2, top2.Item2);

                for (int jj = miny; jj < maxy; jj++)
                {
                    int y = jj;
                    int x = (int)((-c - b * y) / a);
                    if (y >= w) y = w - 2;
                    if (x >= 480) x = 478;
                    int index = x * w * 4 + y * 4;
                    /*
                    Console.Write(x);
                    Console.Write(' ');
                    Console.Write(y);
                    Console.WriteLine();
                    */ 
                    pixeldata[index] = 255;
                    pixeldata[index + 1] = 255;
                    pixeldata[index + 2] = 255;
                    pixeldata[index + 3] = 255;
                }
            }
        }

        void drawRectangle1(byte[] pixeldata, int w, int leftTopX, int leftTopY, int rightTopX, int rightTopY)
        {
            for (int ii = leftTopX; ii < rightTopX; ii++)
            {
                int jj1 = leftTopY;
                int jj2 = rightTopY;
                int index1 = ii * w * 4 + jj1 * 4;
                pixeldata[index1] = 255;
                pixeldata[index1 + 1] = 255;
                pixeldata[index1 + 2] = 255;
                pixeldata[index1 + 3] = 255;

                int index2 = ii * w * 4 + jj2 * 4;
                pixeldata[index2] = 255;
                pixeldata[index2 + 1] = 255;
                pixeldata[index2 + 2] = 255;
                pixeldata[index2 + 3] = 255;

            }
            for (int jj = leftTopY; jj < rightTopY; jj++)
            {
                int ii1 = leftTopX;
                int ii2 = rightTopX;
                int index3 = ii1 * w * 4 + jj * 4;
                pixeldata[index3] = 255;
                pixeldata[index3 + 1] = 255;
                pixeldata[index3 + 2] = 255;
                pixeldata[index3 + 3] = 255;

                int index4 = ii2 * w * 4 + jj * 4;
                pixeldata[index4] = 255;
                pixeldata[index4 + 1] = 255;
                pixeldata[index4 + 2] = 255;
                pixeldata[index4 + 3] = 255;
            }
        }
        void writeDataToFile(byte[] pixeldata, int w, int h, string name)
        {
            Console.WriteLine(name);
            System.IO.StreamWriter file = new System.IO.StreamWriter(name);
            file.WriteLine("a(:,:,1)= [");

            for (int xx = 0; xx < h; xx++)
            {
                for (int yy = 0; yy < w * 4; yy += 4)
                {
                    string value = pixeldata[xx * w * 4 + yy].ToString();

                    int length = 3 - value.Length;
                    for (int i = 0; i < length; i++)
                        value = " " + value;

                    file.Write(value);
                    file.Write(' ');
                }
                if (xx != h - 1)
                    file.WriteLine(';');
                else
                    file.WriteLine("];");
            }

            file.WriteLine();
            file.WriteLine();
            file.WriteLine("a(:,:,2)=[");

            for (int xx = 0; xx < h; xx++)
            {
                for (int yy = 1; yy < w * 4; yy += 4)
                {
                    string value = pixeldata[xx * w * 4 + yy].ToString();

                    int length = 3 - value.Length;
                    for (int i = 0; i < length; i++)
                        value = " " + value;

                    file.Write(value);
                    file.Write(' ');
                }
                if (xx != h - 1)
                    file.WriteLine(';');
                else
                    file.WriteLine("];");
            }
            file.WriteLine();
            file.WriteLine();

            file.WriteLine("a(:,:,3)=[");

            for (int xx = 0; xx < h; xx++)
            {
                for (int yy = 2; yy < w * 4; yy += 4)
                {
                    string value = pixeldata[xx * w * 4 + yy].ToString();

                    int length = 3 - value.Length;
                    for (int i = 0; i < length; i++)
                        value = " " + value;

                    file.Write(value);
                    file.Write(' ');
                }
                if (xx != h - 1)
                    file.WriteLine(';');
                else
                    file.WriteLine("];");
            }
            file.WriteLine();
            file.WriteLine();
            /*
            file.WriteLine("a(:,:,4)=[");

            for (int xx = 0; xx < h; xx++)
            {
                for (int yy = 2; yy < w * 4; yy += 4)
                {
                    string value = pixeldata[xx * w*4 + yy].ToString();

                    int length = 3 - value.Length;
                    for (int i = 0; i < length; i++)
                        value = " " + value;

                    file.Write(value);
                    file.Write(' ');
                }
                if (xx != h - 1)
                    file.WriteLine(';');
                else
                    file.WriteLine("];");
            }
                
            */
            file.Close();
        
        }
        void convertEdgeVertical(byte[] pixeldata, int w, int h)
        { 
            double[,] columnMask = new double[3,3] { {1,0,-1},
                                                     {2,0,-2},
                                                     {1,0,-1}};
            byte[] temp = new byte[w*h*4];

            for (int i = 0; i < w * h * 4; i++)
                temp[i] = pixeldata[i];

                    
                    
            for(int i=1; i< h-1; i++)
                for (int j = 4; j < 4*(w - 1); j++)
                {
                    double sum = 0;
                    for(int ii=0;ii<3;ii++)
                        for (int jj = 0; jj < 3; jj++)
                        {
                            int x = i+ii-1;
                            int y = j+(jj-1)*4;
                            int index = x * w *4 + y;
                            /*
                            Console.Write(x);
                            Console.Write(' ');
                            Console.Write(y);
                            Console.WriteLine();
                              */
                            sum += columnMask[ii, jj] * temp[index];
                        }
                    sum = Math.Abs(sum);
                    if (Math.Round(sum / 4, 0) > 254)
                        pixeldata[i * 4* w + j] = 255;
                    if (Math.Round(sum / 4, 0) < 0)
                        pixeldata[i * 4 * w + j] = 0;
                    else
                        pixeldata[i * w * 4 + j] = (byte)(Math.Round(sum / 4, 0));
                    if (pixeldata[i * w * 4 + j] > EDGE_COLOR)
                        pixeldata[i * w * 4 + j] = 255;
                }            
        }
        void convertEdgeHorizontal(byte[] pixeldata, int w, int h)
        {
            double[,] rowMask = new double[3, 3] { {1, 2, 1},
                                                      {0, 0, 0},
                                                     {-1,-2,-1}};
            byte[] temp = new byte[w * h * 4];

            for (int i = 0; i < w * h * 4; i++)
                temp[i] = pixeldata[i];



            for (int i = 1; i < h - 1; i++)
                for (int j = 4; j < 4 * (w - 1); j++)
                {
                    double sum = 0;
                    for (int ii = 0; ii < 3; ii++)
                        for (int jj = 0; jj < 3; jj++)
                        {
                            int x = i + ii - 1;
                            int y = j + (jj - 1) * 4;
                            int index = x * w * 4 + y;
                            /*
                            Console.Write(x);
                            Console.Write(' ');
                            Console.Write(y);
                            Console.WriteLine();
                              */
                            sum += rowMask[ii, jj] * temp[index];
                        }
                    sum = Math.Abs(sum);
                    if (Math.Round(sum / 4, 0) > 254)
                        pixeldata[i * 4 * w + j] = 255;
                    if (Math.Round(sum / 4, 0) < 0)
                        pixeldata[i * 4 * w + j] = 0;
                    else
                        pixeldata[i * w * 4 + j] = (byte)(Math.Round(sum / 4, 0));
                    if (pixeldata[i * w * 4 + j] > EDGE_COLOR)
                        pixeldata[i * w * 4 + j] = 255;
                }
        }
        void getRed(byte[] pixeldata, int w, int h, byte[,] red)
        {
            for (int i = 0; i < h; i++)
                for (int j = SELECTED_COLOR; j < 4 * w; j += 4)
                    red[i, (int)j/4] = pixeldata[i * w * 4 + j];
        }
        int countEdge(byte[,] data, int w, int h, int x, int y)
        {
            int number =0;
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    if ((x + i - 1 < h) && (y + j - 1 < w) && (x + i - 1 >= 0) && (y + j - 1 >= 0))
                        if ( data[x + i - 1, y + j - 1] == 255 )
                            number++;
            return number;
        }
        // xac dinh canh doc dai nhat.
        // chi xet 1/4 cua man hinh nen yeu cau goc tren trai phai nam o 1/4 cua man hinh.
        Tuple<int, int, int, int> detectEdgeVertical(byte[,] data, int w, int h)
        {
            int maxLength = 0;
            int top1X=0,top1Y=0,top2X=0,top2Y=0 ;

            for(int i =1; i < h/2; i++)
                for (int j = 1; j < w/2; j++)
                {
                    int x =i;
                    int y =j;
                    //de phat hien cac duong thang nghieng.
                    for (double hsGoc = -0.01; hsGoc < 0.1; hsGoc += 0.01)
                    {
                        while (countEdge(data, w, h, x, y) >=1 )
                        {
                            x++;
                            y = (int)(j + Math.Round((x - i + 1) * hsGoc, 1));
                            /*
                            Console.Write(x);
                            Console.Write(' ');
                            Console.Write(y);
                            Console.WriteLine();
                             */ 
                        }
                        if (x - i > maxLength)
                        {
                            maxLength = x - i;
                            top1X = i;
                            top1Y = j;
                            top2X = x;
                            top2Y = y;
                        }
                    }
                }
            return new Tuple<int, int, int, int>(top1X, top1Y, top2X, top2Y);
        }
        Tuple<int, int, int, int> detectEdgeHorizontal(byte[,] data, int w, int h,int maxH,int maxW)
        {
            int maxLength = 0;
            int top1X = 0, top1Y = 0, top2X = 0, top2Y = 0;

            for (int i = 1; i < maxH+20; i++)
                for (int j = maxW-20; j < w; j++)
                {
                    int x = i;
                    int y = j;
                    //Console.WriteLine("x=");
                    for (double hsGoc = -0.01; hsGoc < 0.1; hsGoc += 0.01)
                    {
                        while (countEdge(data, w, h, x, y) >= 1)
                        {
                            y++;
                            x = (int)(i + Math.Round((y - j + 1) * hsGoc, 1));
                            /*
                            Console.Write(x);
                            Console.Write(' ');
                            Console.Write(y);
                            Console.WriteLine();
                             */
                        }
                        if (y - j > maxLength)
                        {
                            maxLength = y - j;
                            top1X = i;
                            top1Y = j;
                            top2X = x;
                            top2Y = y;
                        }
                    }
                }
            return new Tuple<int, int, int, int>(top1X, top1Y, top2X, top2Y);
        }
        Tuple<double, double> ghpt(double a1, double b1, double c1,double a2, double b2, double c2)
        {
            double d = a1 * b2 - a2 * b1;
            double dx = c1 * b2 - c2 * b1;
            double dy = a1 * c2 - a2 * c1;
            return new Tuple<double, double>(-dx / d, -dy / d);
        }
        void findRectangle(Tuple<int, int, int, int> lineV, Tuple<int, int, int, int> lineH)
        {
            // tim giao diem cua 1 duong ngang va doc
            double a1 = lineV.Item4 - lineV.Item2;
            double b1 = lineV.Item1 - lineV.Item3;
            double c1 = lineV.Item3 * lineV.Item2 - lineV.Item1 * lineV.Item4;

            double a2 = lineH.Item4 - lineH.Item2;
            double b2 = lineH.Item1 - lineH.Item3;
            double c2 = lineH.Item3 * lineH.Item2 - lineH.Item1 * lineH.Item4;

            Tuple<double, double> top = ghpt(a1, b1, c1, a2, b2, c2);

            top1 = new Tuple<int, int>((int) top.Item1, (int)top.Item2);
            top2 = new Tuple<int, int>(lineV.Item3, lineV.Item4);
            top4 = new Tuple<int, int>(lineH.Item3, lineH.Item4);

            // tim diem thu 4 khi biet 3 diem cua hinh.
            double mx = (top1.Item1 + top4.Item1) / 2.0;
            double my = (top1.Item2 + top4.Item2) / 2.0; 
            a1 = top1.Item1 - top4.Item1;
            b1 = top1.Item2 - top4.Item2;
            c1 = -(a1 * mx + b1 * my);

            a2 = -b1;
            b2 = a1;
            c2 = -(a2 * top2.Item1 + b2 * top2.Item2);
            top = ghpt(a1, b1, c1, a2, b2, c2);


            int top3x = (int)(2*top.Item1  - top2.Item1) ;
            int top3y = (int)(2*top.Item2  - top2.Item2) ;
            top3 = new Tuple<int, int>(top3x, top3y);
        }
        void writeResult()
        {
            drawLine(pixeldata, w, top1, top2);
            drawLine(pixeldata, w, top2, top3);
            drawLine(pixeldata, w, top3, top4);
            drawLine(pixeldata, w, top4, top1);

            SkeletonPoint depthPoint = depthImage.MapToSkeletonPoint(top1.Item2, top1.Item1);

            //SkeletonPoint depthPoint = depthImage.MapToSkeletonPoint(320,240 );
            textTop1.Text = top1.Item1.ToString() + " " + top1.Item2.ToString() + " " + Math.Round(depthPoint.X,2) + " " + Math.Round(depthPoint.Y,2) + " " + Math.Round(depthPoint.Z,2);

            depthPoint = depthImage.MapToSkeletonPoint(top2.Item2, top2.Item1);
            textTop2.Text = top2.Item1.ToString() + " " + top2.Item2.ToString() + " " + Math.Round(depthPoint.X,2) + " " + Math.Round(depthPoint.Y,2) + " " + Math.Round(depthPoint.Z,2); 

            depthPoint = depthImage.MapToSkeletonPoint(top3.Item2, top3.Item1);
            textTop3.Text = top3.Item1.ToString() + " " + top3.Item2.ToString() + " " + Math.Round(depthPoint.X,2) + " " + Math.Round(depthPoint.Y,2) + " " + Math.Round(depthPoint.Z,2); 

            depthPoint = depthImage.MapToSkeletonPoint(top4.Item2, top4.Item1);
            textTop4.Text = top4.Item1.ToString() + " " + top4.Item2.ToString() + " " + Math.Round(depthPoint.X,2) + " " + Math.Round(depthPoint.Y,2) + " " + Math.Round(depthPoint.Z,2); 
        }
        BitmapSource ImageToBitmap(ColorImageFrame Image)
        {
            Image.CopyPixelDataTo(pixeldata);
            w = Image.Width;
            h = Image.Height;
            /*
            int leftTopX = 183;
            int leftTopY = 230;
            int rightTopX = 350;
            int rightTopY = 424;
            */  
            //drawRectangle(pixeldata, w, leftTopX, leftTopY, rightTopX, rightTopY);
            Console.WriteLine(count);
            if ((count > 60) && (count < 1000))
            {
                writeDataToFile(pixeldata, w, h, "D:\\test4.txt");
                int t = 0;
                //count = 2000;
            }
            convertEdgeVertical(pixeldata, w, h);
            getRed(pixeldata, w, h, red);
            lineV = detectEdgeVertical(red, w, h);

            //drawRectangle(pixeldata, w, leftTopX, leftTopY, rightTopX, rightTopY);
            Image.CopyPixelDataTo(pixeldata);
            convertEdgeHorizontal(pixeldata, w, h);
            getRed(pixeldata, w, h, red);
            lineH = detectEdgeHorizontal(red, w, h,lineV.Item1,lineV.Item2);

            findRectangle(lineV, lineH);
            Image.CopyPixelDataTo(pixeldata);
            findRectangle(lineV, lineH);
            writeResult();
            //drawRectangle1(pixeldata, w, lineV.Item1  , lineV.Item2, lineV.Item3 , lineV.Item4);
            //drawRectangle1(pixeldata, w, lineH.Item1 , lineH.Item2, lineH.Item3 , lineH.Item4);

            //drawRectangle1(pixeldata, w, top1.Item1, top1.Item2, top2.Item1, top2.Item2);
            //drawRectangle1(pixeldata, w, top2.Item1, top2.Item2, top3.Item1, top3.Item2);
            //drawRectangle1(pixeldata, w, top4.Item1, top4.Item2, top3.Item1, top3.Item2);
            //drawRectangle1(pixeldata, w, top1.Item1, top1.Item2, top4.Item1, top4.Item2);

            if ((count > 60) && (count < 1000))
            {
                writeDataToFile(pixeldata, w, h,"D:\\test5.txt");
                int t = 0;
                count = 2000;
            }                      
            BitmapSource bmap = BitmapSource.Create(
             Image.Width,
             Image.Height,
             96, 96,
             PixelFormats.Bgr32,
             null,
             pixeldata,
             Image.Width * Image.BytesPerPixel);
            return bmap;
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            sensor.Stop();
        }

        private void button1_Click(object sender, RoutedEventArgs e)
        {
            sensor.ElevationAngle += 4;            
        }

        private void button2_Click(object sender, RoutedEventArgs e)
        {
            sensor.ElevationAngle -= 4;
        }
        private void stopDetect(object sender, RoutedEventArgs e)
        {
            isStop = !isStop;
            if (isStop)
                stop.Content = "continue";
            else
                stop.Content = "stop";
        }

    }
}